#include "kalmanfilter.h"

KalmanFilter::KalmanFilter(){}
KalmanFilter::KalmanFilter(const Eigen::VectorXd& initial_state, const Eigen::MatrixXd& initial_covariance, const Eigen::MatrixXd& process_noise_cov, const Eigen::MatrixXd& measurement_noise_cov, const Eigen::MatrixXd& state_transition_matrix, const Eigen::MatrixXd& measurement_matrix)
    : x_kalm_(initial_state), p_kalm_(initial_covariance), Q_(process_noise_cov),
      R_(measurement_noise_cov), F_(state_transition_matrix), H_(measurement_matrix) {}

KalmanFilter::KalmanFilter(const Eigen::VectorXd& initial_state, const Eigen::MatrixXd& initial_covariance, const Eigen::MatrixXd& process_noise_cov, const Eigen::MatrixXd& measurement_noise_cov, const Eigen::MatrixXd& measurement_matrix)
    : x_kalm_(initial_state), p_kalm_(initial_covariance), Q_(process_noise_cov),
      R_(measurement_noise_cov), H_(measurement_matrix) {}

KalmanFilterResult KalmanFilter::filter(const Eigen::VectorXd& z_) {
    // 预测
    x_pred = F_ * x_kalm_;
    p_pred = F_ * p_kalm_ * F_.transpose() + Q_;
    // 计算Kalman增益
    K = p_pred * H_.transpose() * (H_ * p_pred * H_.transpose() + R_).inverse();
    // 更新
    x_kalm_ = x_pred + K * (z_ - H_ * x_pred);
    p_kalm_ = (Eigen::MatrixXd::Identity(F_.rows(), F_.cols()) - K * H_) * p_pred;

    KalmanFilterResult result;
    result.state = x_kalm_;
    result.covariance = p_kalm_;

    return result;
}
KalmanFilterResult KalmanFilter::filter(const Eigen::VectorXd& z_, const Eigen::MatrixXd& state_transition_matrix){
    F_ = state_transition_matrix;
    // 预测
    x_pred = F_ * x_kalm_;
    p_pred = F_ * p_kalm_ * F_.transpose() + Q_;
    // 计算Kalman增益
    K = p_pred * H_.transpose() * (H_ * p_pred * H_.transpose() + R_).inverse();
    // 更新
    x_kalm_ = x_pred + K * (z_ - H_ * x_pred);
    p_kalm_ = (Eigen::MatrixXd::Identity(F_.rows(), F_.cols()) - K * H_) * p_pred;

    KalmanFilterResult result;
    result.state = x_kalm_;
    result.covariance = p_kalm_;

    return result;
}